#!/usr/bin/python2.7
# -*- coding: utf-8 -*-
import socket
from threading import Thread
import struct
import re
import time
import pickle
import numpy as np
import rospy
import rospkg
from std_msgs.msg import Bool
from px4_gcs.msg import swarm_state
from enum import Enum
global msg_len
msg_len = 100
time_num =1598419990910
global UAV_ID
UAV_ID = 1
global recv_trj_flag
recv_trj_flag = False
global swarm_state_ready
swarm_state_ready = 0


trj_recv = []

client_test = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
print('建立客户端套接字')
host = '202.121.181.221'
port = 2333
client_test.connect((host,port))
print('连接到服务器')

def trj_recv_f():
	global msg_len
	global recv_trj_flag
	global data_number
	rospack = rospkg.RosPack()
	rospack.list()
	pkg_path = rospack.get_path('px4_gcs')
	while True:
		client_require_send = [11,222,-1,0.1,0.1]
		client_require_sendpack = struct.pack('!3i2f',*client_require_send)
		if len(trj_recv) < msg_len:
			print(len(trj_recv),msg_len,)
			if recv_trj_flag == False:
				client_test.send(client_require_sendpack)
				print('发送次数')
			print('数据未完全接受，继续发送请求')
			client_recv_msg = client_test.recv(20)
			client_ID,data_number,msg_len,pose_x,pose_y = struct.unpack('!3i2f',client_recv_msg)
			if data_number > 0:
				recv_trj_flag = True
				print(recv_trj_flag)
			print('收到server数据！')
			print(client_ID,data_number,msg_len,pose_x,pose_y)
			if client_ID:
				if data_number == -100:
					print("server 未完全接收轨迹点！")
				else:
					trj_recv.append([client_ID,data_number,msg_len,pose_x,pose_y])
		if len(trj_recv) == msg_len:
			print('开始写入trj！')
			np.savetxt(pkg_path+'/traj/trj1_recv.csv',trj_recv,delimiter=',')
			print('写入完毕！！')
			break


def mavros_state_sub(mavrosState):
	global count
	if count!=2:
		count += 1
		global swarm_state_ready
		if mavrosState.data == True:
			#这个输入后 在server中的判断要注意区别收到消息的时效性 有可能是client断开了
			state_send = [UAV_ID,1,9999999,0.0,0.0]
			state_send_pack = struct.pack('!3i2f',*state_send)
			client_test.send(state_send_pack)
			print('无人机 1 已经准备好!')
			state_recv = client_test.recv(20)
			client_ID,data_number,msg_len,pose_x,pose_y = struct.unpack('!3i2f',state_recv)
			swarm_state_ready = data_number
			print(data_number,msg_len)
			if msg_len == 9999999 and data_number == 28:
				print('所有无人机准备好!')
			elif msg_len == 9999999 and data_number == 24:
				print('无人机2未准备好!')
			elif msg_len == 9999999 and data_number == 20:
				print('无人机1未准备好!')
			elif msg_len == 9999999 and data_number == 12:
				print('无人机0未准备好!')
			elif msg_len == 9999999 and data_number == 0:
				print('至少两架无人机未准备好!')
		elif mavrosState.data == False:
			state_send = [UAV_ID,-1,9999999,0.0,0.0]
			state_send_pack = struct.pack('!3i2f',*state_send)
			client_test.send(state_send_pack)
			print('无人机 1 未准备好!')
			state_recv = client_test.recv(20)
			client_ID,data_number,msg_len,pose_x,pose_y = struct.unpack('!3i2f',state_recv)
			swarm_state_ready = data_number
			if msg_len == 9999999 and data_number == 28:
				print('所有无人机准备好!')
			elif msg_len == 9999999 and data_number == 24:
				print('无人机2未准备好!')
			elif msg_len == 9999999 and data_number == 20:
				print('无人机1未准备好!')
			elif msg_len == 9999999 and data_number == 12:
				print('无人机0未准备好!')
			elif msg_len == 9999999 and data_number == 0:
				print('至少两架无人机未准备好!')
	else:
		count = 0

def swarm_state_pub():
	swarm_state_pub = rospy.Publisher('/swarm/state',swarm_state,queue_size = 1)
	rate = rospy.Rate(50)
	swarm_state_msg = swarm_state()
	swarm_state_msg.traj_ready = True
#	swarm_state_list = list(swarm_state.ready)
#	print(swarm_state_list)
	while len(swarm_state_msg.ready) < 3:
		swarm_state_msg.ready.append(0)
	while len(swarm_state_msg.nodes_id) < 3:
		swarm_state_msg.nodes_id.append(0)
	while len(swarm_state_msg.active) < 3:
		swarm_state_msg.active.append(1)
	while not(rospy.is_shutdown()):
		swarm_state_msg.ready[0] = 1 if (swarm_state_ready & 16)==16 else 0# = 1 if (swarm_state_ready & 8)==8 else 0
		swarm_state_msg.ready[1] = 1 if (swarm_state_ready & 8)==8 else 0
		swarm_state_msg.ready[2] = 1 if (swarm_state_ready & 4)==4 else 0
		print(swarm_state_msg.ready[0],swarm_state_msg.ready[1],swarm_state_msg.ready[2])
		swarm_state_msg.master_id = 0
		swarm_state_msg.nodes_id[1] = 1
		swarm_state_pub.publish(swarm_state_msg)
		rate.sleep()

if __name__ == '__main__':
	trj_recv_f()
	rospy.init_node('client1',anonymous = True)
	global count
	count = 0
	rospy.Subscriber('/mavros/ready',Bool,mavros_state_sub,queue_size = 1)
	swarm_state_pub()
#	swarm_state_list = list(swarm_state.ready)
#	print(swarm_state_list)	
		#print("swarm_state_msg已经发布")
#	rospy.Subscriber('/mavros/ExtenedState',mavros_msgs/ExtenedState Message,extend_state_sub,queuesize = 1)

	rospy.spin()



#	print('Request后，接收到的客户端消息：',client_ID,data_number,msg_len,pose_x,pose_y)
#	f_dataa,f_datab,f_datac,f_datad = struct.unpack('!4f',client_recv_msg)
#	print('接收到的客户端消息：',f_dataa,f_datab,f_datac,f_datad) 


